//------------------------------------------------------------------------------
//  <copyright file="FRIServerTest.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once

#include "stdafx.h"
#include "KukaLBR4HardwareTests.h"
#include "KukaLBR4NativeTestLibImportExport.h"

#include <IFRIRemote.h>
#include <FriServer.h>
#include <friComm.h>

class SimpleMockFRIRemote : public IFRIRemote
{
public:
	/// <summary> Initializes a new instance of the SimpleMockFRIRemote controller class </summary>
	SimpleMockFRIRemote(int port = DEFAULT_FRI_SERVER_PORT, char * hintToRemoteHost = 0);

	/// <summary> dtor </summary>
	~SimpleMockFRIRemote() { };

	/// <summary> Excange data with FRI client.  Blocks on next packet from FRI client. </summary>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doDataExchange();

	/// <summary> Receive data from FRI client.  Blocks on next packet from FRI client. </summary>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doReceiveData();

	/// <summary> Send data to the FRI client.  </summary>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doSendData();

	/// <summary> Execute a position control command. </summary>
	/// <param name="newJntPosition"> Target joint position. </param>
	/// <param name="flagDataExchange"> 
	/// Indicates whether or not to automatically exchange data with the FRI client before returning.
	/// </param>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange=true);

	/// <summary> Executes a joint impdance control command. </summary>
	/// <param name="newJntPosition"> Joint positions.  Null value ignored. </param>
	/// <param name="newJntStiff"> Joint stiffness.  Null value ignored. </param>
	/// <param name="newJntDamp"> Joint damping.  Null value ignored.   </param>
	/// <param name="newJntAddTorque"> Additional torque.  Null values ignored. </param>
	/// <param name="flagDataExchange"> 
	/// Indicates whether or not to automatically exchange data with the FRI client before returning.
	/// </param>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doJntImpedanceControl(
		const float newJntPosition[LBR_MNJ], 
		const float newJntStiff[LBR_MNJ] = NULL,
		const float newJntDamp[LBR_MNJ]=NULL,
		const float newJntAddTorque[LBR_MNJ]=NULL,
		bool flagDataExchange=true);

	/// <summary> Execute a cartesion impedance command </summary>
	/// <param name="newCartStiff"> Cartesian stiffness. Null value ignored. </param>
	/// <param name="newCartDam"> Cartesian damping.  Null value ignored. </param>
	/// <param name="newAddTcpFT> Additional force and torque on the TCP. </param>
	/// <param name="newJntNullspace"> Joint nullspace. </param>
	/// <param name="flagDataExchange"> 
	/// Indicates whether or not to automatically exchange data with the FRI client before returning.
	/// </param>
	/// <returns> 0 on success.  -1 on failure. </returns>
	int doCartesianImpedanceControl(const float newCartPosition[FRI_CART_FRM_DIM], 
		const float newCartStiff[FRI_CART_VEC]=NULL, 
		const float newCartDamp[FRI_CART_VEC]=NULL, 
		const float newAddTcpFT[FRI_CART_VEC]=NULL,
		const float newJntNullspace[LBR_MNJ]=NULL,
		bool flagDataExchange=true);

	/// <summary> Get the state of the FRI connection. </summary>
	/// <returns> State of the FRI connection. </returns>
	FRI_STATE getState();

	/// <summary> Get the quality of the FRI connection. </summary>
	/// <returns> Quality of the FRI connection. </returns>
	FRI_QUALITY getQuality();
	
	/// <summary> Get the current measured joint position. </summary>
	/// <returns> The current measured joint position. </returns>
	float* getMsrMsrJntPosition();

	/// <summary> Get the current measured commanded Cartesian position of TCP. </summary>
	/// <returns> The current measured commanded Cartesian position of TCP. </returns>
	float* getMsrCmdCartPosition();

	/// <summary> Get the current measured Cartesian position of TCP. </summary>
	/// <returns> The current measured Cartesian position of TCP. </returns>
	float* getMsrCartPosition();

	/// <summary> Get real value from KRL.  Corresponds to KRL variable $FRI_TO_REA </summary>
	/// <param name=index> Index of the real value to get </param>
	/// <returns> A float. </returns>
	virtual float getFrmKRLReal(int index) { return 0; }

	/// <summary> Set a real value to KRL. Corresponds to KRL variable $FRI_FRM_REA </summary>
	/// <param name=index> Index of the real value to set. </param>
	/// <param name=val> Value to set. </param>
	virtual void setToKRLReal(int index, float val);

	/// <summary> Get integer value from KRL.  Corresponds to KRL variable $FRI_TO_INT </summary>
	/// <param name=index> Index of the integer value to get </param>
	/// <returns> An int. </returns>
	virtual int getFrmKRLInt(int index) { return 0; }

	/// <summary> Set a integer value to KRL. Corresponds to KRL variable $FRI_FRM_INT </summary>
	/// <param name=index> Index of the integer value to set. </param>
	/// <param name=val> Value to set. </param>
	virtual void setToKRLInt(int index, int val);

	/// <summary> Get bool value from KRL.  Corresponds to KRL variable $FRI_TO_BOOL </summary>
	/// <param name=index> Index of the bool value to get </param>
	/// <returns> A bool. </returns>
	virtual bool getFrmKRLBool(int index) { return false; }

	/// <summary> Get all bool values from KRL.  Corresponds to KRL variable $FRI_TO_BOOL </summary>
	/// <returns> A 16-bit value corresponding to all bool values in $FRI_TO_BOOL bool. </returns>
	virtual fri_uint16_t getFrmKRLBool() { return 0; }

	/// <summary> Set a bool value to KRL. Corresponds to KRL variable $FRI_FRM_BOOL </summary>
	/// <param name=index> Index of the bool value to set. </param>
	/// <param name=val> Value to set. </param>
	virtual void  setToKRLBool(int index, bool val) { }

	/// <summary> Set all bool values to KRL. Corresponds to KRL variable $FRI_FRM_BOOL </summary>
	/// <param name=val> A 16-bit value corresponding to all bool values to set to $FRI_FRM_BOOL bool. </param>
	virtual void setToKRLBool(fri_uint16_t val) { }

	/// <summary> Get the data stored in the msr buffer. </summary>
	/// <returns> The data stored in the msr buffer. </returns>
	tFriMsrData & getMsrBuf();

	/// <summary> The data returned by <c>getMsrBuf</c> and getMsrMsrJntPosition </summary>
	tFriMsrData FRIMsrData;

	/// <summary> The quality that will be returned by <c>getQuality</c> </summary>
	FRI_QUALITY  FRIQuality;

	/// <summary>
	/// The joint positions expected by any call that takes a joint position.
	/// Other values will cause an exception.
	/// </summary>
	float expectedJointPosition[LBR_MNJ];

	/// <summary>
	/// The Cartesian frame expected by any call that accepts a Cartesian frame
	/// as a parameter.  Other values will cause an exception.
	/// </summary>
	float expectedCartesianPosition[CARTESIAN_FRAME_LENGTH];

	/// <summary>
	/// Expected values being passed in the ToKRLReal registers.
	/// </summary>
	float expectedToKRLReal[FRI_USER_SIZE];

	/// <summary>
	/// Expected values being passed in the ToKRLInt registers.
	/// </summary>
	int expectedToKRLInt[FRI_USER_SIZE];

	/// <summary>
	/// Gets set to true whenever a joint position command is received.
	/// Is never reset by mock.
	/// </summary>
	bool jointPositionCommandReceived;

	/// <summary>
	/// Gets set to true whenever a Cartesian position command is received.
	/// Is never reset by mock.
	/// </summary>
	bool cartesianPositionCommandReceived;

	/// <summary>
	/// Count of calls to doDataExchange();
	/// </summary>
	int doDataExchangeCount;

	/// <summary>
	/// Count of calls to doReceiveData();
	/// </summary>
	int doReceiveDataCount;
	
	/// <summary>
	/// Count of calls to doSendData();
	/// </summary>
	int doSendDataCount;

	/// <summary>
	/// Count of calls to doPositionControl();
	/// </summary>
	int doPositionControlCount;

	/// <summary>
	/// Count of calls to doJntImpedanceControl();
	/// </summary>
	int doJntImpedanceControlCount;

	/// <summary>
	/// Count of calls to doCartesianImpedanceControl();
	/// </summary>
	int doCartesianImpedanceControlCount;

	/// <summary>
	/// Count of calls to getState();
	/// </summary>
	int getStateCount;

	/// <summary>
	/// Count of calls to getQuality();
	/// </summary>
	int getQualityCount;

	/// <summary>
	/// Count of calls to getMsrMsrJntPosition();
	/// </summary>
	int getMsrMsrJntPositionCount;
	
	/// <summary>
	/// Count of calls to getMsrCmdCartPosition();
	/// </summary>
	int getMsrCmdCartPositionCount;

	/// <summary>
	/// Count of calls to getMsrCartPosition();
	/// </summary>
	int getMsrCartPositionCount;

	/// <summary>
	/// Count of calls to getMsrBuf();
	/// </summary>
	int getMsrBufCount;
};
